/*
 *
 ****************************************************************************
 * Copyright (C) 2021 QST Corporation. <www.qstcorp.com>
 * All rights reserved.
 *
 * File : qmp6989.c
 *
 * Date : 2021/06/05
 *
 * Usage: QMP6989 sensor driver header file
 *
 ****************************************************************************
 *
 */

/*! @file qmp6989.c
 *  @brief  QMP6989 Sensor Driver File
 *  @author
 */
#include "n32g031.h"
#include "qmp6989.h"
#include <stdio.h>
#include "DevicesSoftIIC.h"
#include "DevicesDelay.h"

#define QMP6989_DEBUG	printf

#define QMP6989_CALIBRATION_DATA_CNT	(16)
#define WAIT_FOR_DRDY_LOOP_DELAY(count) {int i;for(i = 0; i < (count); ++i);}

static const int32_t QMP6989_POWER_SCALE[] = {1, 10, 100, 1000};
#ifdef FLOAT_SUPPORT
static const float QMP6989_CALIB_SCALE_FACTOR[] = {
  1.0E+00,
  1.0E-05,
  1.0E-10,
  1.0E-05,
  1.0E-10,
  1.0E-15,
  1.0E-12,
  1.0E-17,
  1.0E-21 };

static float fCalibParam[QMP6989_CALIBRATION_PARAMETER_COUNT], fT_Celsius, fP_Pa;//, fAlt_m;
#else
static int32_t int32_tT_Celsius, int32_tP_Pa;
static int16_t int16_tValue[QMP6989_CALIBRATION_PARAMETER_COUNT];
static uint8_t uint8_tPower[QMP6989_CALIBRATION_PARAMETER_COUNT];
#endif
static int32_t int32_tP = 0;
static int16_t int16_tT = 25*256;
static uint16_t qmp6989_uuid;

static qmp6989_mode qmp6989_curr_mode = QMP6989_MODE_OFF;

// QMP6989

/*!
 * @brief Read multiple data from the starting regsiter address
 *
 * @param uint8_tAddr Starting register address
 * @param puint8_tData The data array of values read
 * @param uint8_tLen Number of bytes to read
 *
 * @return Result from the burst read function
 * @retval >= 0 Success
 * @retval -127 Error null bus
 * @retval < 0  Communication error
 *
 */
void qmp6989_delay1ms(int8_t cnt)
{
	vDelayMs(cnt);
}

int8_t qmp6989_burst_read(uint8_t uint8_tAddr, uint8_t* puint8_tData, uint8_t uint8_tLen)
{
	int8_t comRslt = -1;
	uint8_t ret = 0;

#if defined(QMP6989_USE_SPI)
	uint8_t i=0;
	for(i=0; i<uint8_tLen; i++)
	{
		ret += qmp6989_spi_read(uint8_tAddr+i, &puint8_tData[i], 1);
	}
	if(ret >= 1)
		comRslt = 1;
	else
		comRslt = 0;
#else
	ret = cSoftIIC6998ReadDatas(QMP6989_8BIT_I2C_ADDR, uint8_tAddr, puint8_tData, uint8_tLen);
	if(ret != 0)
		comRslt = -1;
	else
		comRslt = 1;
#endif
    return comRslt;
}



/*!
 * @brief Write multiple data to the starting regsiter address
 *
 * @param uint8_tAddr Starting register address
 * @param puint8_tData The data array of values to write
 * @param uint8_tLen Number of bytes to write
 *
 * @return Result from the burst write function
 * @retval >= 0 Success
 * @retval -127 Error null bus
 * @retval < 0   Communication error
 *
 */
int8_t qmp6989_burst_write(uint8_t uint8_tAddr, uint8_t* puint8_tData, uint8_t uint8_tLen)
{
	int8_t comRslt = -1;
	uint8_t ret = 0;

#if defined(QMP6989_USE_SPI)
	uint8_t i=0;

	for(i=0; i<uint8_tLen; i++)
	{
		ret += qmp6989_spi_write(uint8_tAddr+i, &puint8_tData[i], 1);
	}
	if(ret >= 1)
		comRslt = 1;
	else
		comRslt = 0;
#else
	ret = cSoftIIC6998WriteDatas(QMP6989_8BIT_I2C_ADDR, uint8_tAddr, puint8_tData, uint8_tLen);
	if(ret != 0)
		comRslt = -1;
	else
		comRslt = 1;
#endif

  	return comRslt;
}


/*!
 * @brief QMP6989 initialize communication bus
 *
 * @param pbus Pointer to the I2C/SPI read/write bus support struct
 *
 * @return Result from bus communication function
 * @retval 0 Success
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
uint8_t qmp6989_get_pid(void){

  int8_t comRslt = -1;
  uint8_t uint8_tData = 0;

  qmp6989_soft_reset();
  qmp6989_delay1ms(100);  // 100ms
#if defined(QMP6989_USE_SPI)
  uint8_tData = 0x81;
  comRslt = qmp6989_burst_write(QMP6989_RST__REG, &uint8_tData, 1);
  qmp6989_delay1ms(50);  // 10ms
#endif
  //Read chip ID
  comRslt = qmp6989_burst_read(QMP6989_REG_PID, &uint8_tData, 1);
  if(comRslt < 0)
  {
	  QMP6989_DEBUG("qmp6989_get_pid fail\n");
  }
  QMP6989_DEBUG("qmp6989_get_pid : 0x%x\n", uint8_tData);

  return uint8_tData;
}

/*!
 * @brief QMP6989 soft reset
 *
 * @param None
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
int8_t qmp6989_soft_reset(void){

  int8_t comRslt = -1;
  uint8_t uint8_tData = QMP6989_SW_RST_SET_VALUE;

  //Set 00h = 0x24
  comRslt = qmp6989_burst_write(QMP6989_RST__REG, &uint8_tData, 1);

  //????,??????
  return comRslt;
}

/*!
 * @brief Get qmp6989 calibration parameters
 *        - Read calibration register AAh~BBh total 18 bytes
 *        - Compose 9 calibration parameters from the 18 bytes
 *
 * @param fCalibParam: the calibration parameter array returned to caller
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
#ifdef FLOAT_SUPPORT
int8_t qmp6989_get_calibration_param(float* fCalibParam){

  uint8_t uint8_tDataBuf[QMP6989_CALIBRATION_REGISTER_COUNT];
  int8_t comRslt;
  int32_t tmp, shift, i;

  //read the calibration registers
  comRslt = qmp6989_burst_read(QMP6989_REG_CALIB00, uint8_tDataBuf, QMP6989_CALIBRATION_REGISTER_COUNT);

  if(comRslt < 0){
    comRslt = -1;
    goto EXIT;
  }

  // Get the parameters
  shift = sizeof(int32_t)*8 - 16;
  for(i = 0; i < QMP6989_CALIBRATION_PARAMETER_COUNT; ++i){
    tmp = ((int32_t)uint8_tDataBuf[2 * i] << 8) + uint8_tDataBuf[2 * i + 1];
    fCalibParam[i] = ((tmp << shift) >> (shift + 2)) * QMP6989_POWER_SCALE[(uint8_tDataBuf[2 * i + 1] & 0x03)] * QMP6989_CALIB_SCALE_FACTOR[i];
  	QMP6989_DEBUG("fCalibParam[%d]=%f\n",i,fCalibParam[i]);
  }

 EXIT:
  return comRslt;
}

/*!
 * @brief Get qmp6989 calibration parameters for fixed-point compensation
 *        - Read calibration register AAh~BBh total 18 bytes
 *        - Return 9 calibration parameters with fixed-point value and power parts
 *
 * @param int16_tValue[]: array of the value part of the calibration parameter
 * @param uint8_tPower[]: array of the power part of the calibration parameter
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
#else
int8_t qmp6989_get_calibration_param_fixed_point(int16_t int16_tValue[], uint8_t uint8_tPower[]){

  uint8_t uint8_tDataBuf[QMP6989_CALIBRATION_REGISTER_COUNT];
  int8_t comRslt;
  int16_t tmp, i;

  //read the calibration registers
  comRslt = qmp6989_burst_read(QMP6989_REG_CALIB00, uint8_tDataBuf, QMP6989_CALIBRATION_REGISTER_COUNT);

  if(comRslt < 0){
    comRslt = -1;
    goto EXIT;
  }

  for(i = 0; i < QMP6989_CALIBRATION_PARAMETER_COUNT; ++i){
    tmp = ((int16_t)uint8_tDataBuf[2 * i] << 8) + uint8_tDataBuf[2 * i + 1];
    int16_tValue[i] = (tmp>>2);
    uint8_tPower[i] = (tmp & 0x03);
  }

 EXIT:
  return comRslt;
}
#endif

/*!
 * @brief qmp6989 initialization
 *        Set AAh ~ ADh to 0x00
 *
 * @param None
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
int8_t qmp6989_initialization(void){
	int8_t comRslt = 0, int8_tTmp;
	uint8_t uint8_tData[] = {0, 0, 0, 0};


#ifdef FLOAT_SUPPORT
	qmp6989_get_calibration_param(fCalibParam);
#else
	qmp6989_get_calibration_param_fixed_point(int16_tValue, uint8_tPower);
#endif
	/* QMP6989 initialization setup */
	//Set AAh ~ AD to 0x00
	int8_tTmp = qmp6989_burst_write(QMP6989_REG_CALIB00, uint8_tData, 4);

	if(int8_tTmp < 0){ //communication error
		comRslt = int8_tTmp;
		goto EXIT;
	}
	comRslt += int8_tTmp;

	qmp6989_set_P_OSR(QMP6989_P_OSR_4096);
	qmp6989_set_T_OSR(QMP6989_T_OSR_1024);
	// after init read sensor data once
	//qmp6989_measure_P(&int32_tP);
	qmp6989_measure_T(&int16_tT);
	qmp6989_set_mode(QMP6989_MODE_P);
 EXIT:
  return comRslt;

}

/*!
 * @brief qmp6989 measure temperature
 *
 * @param *pint16_tT calibrated temperature code returned to caller
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
int8_t qmp6989_measure_T(int16_t* pint16_tT){

  int8_t comRslt = 0, int8_tTmp,int8_ttimecnt = 0;
  uint8_t uint8_tData[2];

  // Set A5h = 0x00, Calibrated data out
  uint8_tData[0] = 0x00;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CONFIG1, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Set 30h = 0x08, T-Forced mode
  uint8_tData[0] = 0x08;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CMD, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Wait for 02h[0] DRDY bit set
  do{

    //wait a while
	   qmp6989_delay1ms(1);

      int8_tTmp = qmp6989_burst_read(QMP6989_REG_STATUS, uint8_tData, 1);

    if(int8_tTmp < 0){ //communication error
      comRslt = int8_tTmp;
      goto EXIT;
    }
    comRslt += int8_tTmp;

  } while(( QMP6989_GET_BITSLICE(uint8_tData[0], QMP6989_DRDY) != 1)&&(int8_ttimecnt++ < 100));

  // Read 09h~0Ah
  int8_tTmp = qmp6989_burst_read(QMP6989_REG_TEMPH, uint8_tData, 2);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Get the calibrated temperature in code
  *pint16_tT = (int16_t)(((int16_t)uint8_tData[0] << 8) + uint8_tData[1]);
	//QMP6989_DEBUG("qmp6989_measure_T, uint8_tData[0]= 0x%x,uint8_tData[1]= 0x%x\n",uint8_tData[0],uint8_tData[1]);
 EXIT:
  return comRslt;
}

/*!
 * @brief qmp6989 measure pressure
 *
 * @param *pint32_tP raw pressure in code returned to caller
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
int8_t qmp6989_measure_P(int32_t* pint32_tP){

  int8_t comRslt = 0, int8_tTmp,int8_ttimecnt = 0;
  uint8_t uint8_tData[3];

  // Set A5h = 0x02, raw data out
  uint8_tData[0] = 0x02;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CONFIG1, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Set 30h = 0x09, P-Forced mode
  uint8_tData[0] = 0x09;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CMD, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Wait for 02h[0] DRDY bit set
  do{

    //wait a while
	  qmp6989_delay1ms(1);

      int8_tTmp = qmp6989_burst_read(QMP6989_REG_STATUS, uint8_tData, 1);

    if(int8_tTmp < 0){ //communication error
      comRslt = int8_tTmp;
      goto EXIT;
    }
    comRslt += int8_tTmp;

  } while(( QMP6989_GET_BITSLICE(uint8_tData[0], QMP6989_DRDY) != 1)&&(int8_ttimecnt++ < 100));

  // Read 06h~08h
  int8_tTmp = qmp6989_burst_read(QMP6989_REG_PRESSH, uint8_tData, 3);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  int8_tTmp = sizeof(*pint32_tP)*8 - 24;
  // Get the raw pressure in code
  *pint32_tP = (int32_t)(((int32_t)uint8_tData[0] << 16) + ((int32_t)uint8_tData[1] << 8) + uint8_tData[2]);
  *pint32_tP = (*pint32_tP << int8_tTmp) >> int8_tTmp; //24 bit sign extension

 EXIT:
  return comRslt;
}

/*!
 * @brief qmp6989 measure pressure and temperature
 *        Read pressure first then commit pressure data conversion for the next call
 *
 * @param *pint32_tP raw pressure in code returned to caller
 * @param *pint16_tT calibrated temperature code returned to caller
 * @param int8_tWaitPDrdy 1: P wait for DRDY bit set, 0: P no wait
 *
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
int8_t qmp6989_measure_P_T(int32_t* pint32_tP, int16_t* pint16_tT, int8_t int8_tPWaitDrdy){

  int8_t comRslt = 0, int8_tTmp,int8_ttimecnt = 0;
  uint8_t uint8_tData[3];

  /*
   *
   * Read raw P code
   *
   */
  if(int8_tPWaitDrdy){
    // Wait for 02h[0] DRDY bit set if int8_tPWaitDrdy is 1
    do{

      //wait a while
	  qmp6989_delay1ms(1);

      int8_tTmp = qmp6989_burst_read(QMP6989_REG_STATUS, uint8_tData, 1);

      if(int8_tTmp < 0){ //communication error
	comRslt = int8_tTmp;
	goto EXIT;
      }
      comRslt += int8_tTmp;

	} while(( QMP6989_GET_BITSLICE(uint8_tData[0], QMP6989_DRDY) != 1)&&(int8_ttimecnt++ < 100));
  }

  // Read 06h~08h
  int8_tTmp = qmp6989_burst_read(QMP6989_REG_PRESSH, uint8_tData, 3);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  int8_tTmp = sizeof(*pint32_tP)*8 - 24;
  // Get the raw pressure in code
  *pint32_tP = ((int32_t)uint8_tData[0] << 16) + ((int32_t)uint8_tData[1] << 8) + uint8_tData[2];
  *pint32_tP = (*pint32_tP << int8_tTmp) >> int8_tTmp; //24 bit sign extension

  /*
   *
   * Measure calibrated T code
   *
   */
  // Set A5h = 0x00, Calibrated data out
  uint8_tData[0] = 0x00;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CONFIG1, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Set 30h = 0x08, T-Forced mode
  uint8_tData[0] = 0x08;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CMD, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;
  int8_ttimecnt	= 0;
  // Wait for 02h[0] DRDY bit set
  do{

    //wait a while
    WAIT_FOR_DRDY_LOOP_DELAY(1000)

      int8_tTmp = qmp6989_burst_read(QMP6989_REG_STATUS, uint8_tData, 1);

    if(int8_tTmp < 0){ //communication error
      comRslt = int8_tTmp;
      goto EXIT;
    }
    comRslt += int8_tTmp;

  } while(( QMP6989_GET_BITSLICE(uint8_tData[0], QMP6989_DRDY) != 1)&&(int8_ttimecnt++ < 100));

  // Read 09h~0Ah
  int8_tTmp = qmp6989_burst_read(QMP6989_REG_TEMPH, uint8_tData, 2);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Get the calibrated temperature in code
  *pint16_tT = (uint8_tData[0] << 8) + uint8_tData[1];

  /*
   *
   * Commit the next pressure conversion
   *
   */
  // Set A5h = 0x02, raw data out
  uint8_tData[0] = 0x02;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CONFIG1, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  // Set 30h = 0x09, P-Forced mode
  uint8_tData[0] = 0x09;
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CMD, uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

 EXIT:
  return comRslt;
}
float qmp6989_calibration_process(float pfS_Pa, float pfP_Pa)
{
	static float pfo_Pa = 0.0f;
	static uint8_t uint8_tCnt = 0;

	if(uint8_tCnt < QMP6989_CALIBRATION_DATA_CNT)
	{
		pfo_Pa += pfP_Pa;
		uint8_tCnt++;
	}
	else
	{
		pfo_Pa =(pfo_Pa/uint8_tCnt - pfS_Pa);
	}

	return pfo_Pa;
}
/*!
 * @brief qmp6989 temperature and pressure compensation
 *
 * @param int16_tT calibrated temperature in code
 * @param int32_tP raw pressure in code
 * @param fParam[] pressure calibration parameters
 * @param *pfT_Celsius calibrated temperature in Celsius returned to caller
 * @param *pfP_Pa calibraated pressure in Pa returned to caller
 *
 * @return None
 *
 */
#ifdef FLOAT_SUPPORT
void qmp6989_compensation(int16_t int16_tT, int32_t int32_tP, float fParam[], float* pfT_Celsius, float* pfP_Pa){

  *pfT_Celsius = QMP6989_T_CODE_TO_CELSIUS(int16_tT);

  *pfP_Pa = \
    fParam[0] + \
    fParam[1]*int16_tT + \
    fParam[2]*int16_tT*int16_tT + \
    fParam[3]*int32_tP + \
    fParam[4]*int16_tT*int32_tP + \
    fParam[5]*int16_tT*int16_tT*int32_tP + \
    fParam[6]*int32_tP*int32_tP + \
    fParam[7]*int16_tT*int32_tP*int32_tP + \
    fParam[8]*int16_tT*int16_tT*int32_tP*int32_tP;

}
#else
#define ShiftRight(v, s) (((v)+(1<<((s)-1)))>>(s))
#define RoundDivide(v, d) (((v)+((d)/2))/(d))

/*!
 * @brief qmp6989 temperature and pressure compensation, s64 fixed point operation
 *
 * @param int16_tT raw temperature in code
 * @param int32_tP raw pressure in code
 * @param int16_tValue[]: array of the value part of the calibration parameter
 * @param uint8_tPower[]: array of the power part of the calibration parameter
 * @param *pint32_tT_Celsius calibrated temperature in 1/256*Celsius returned to caller
 * @param *pint32_tP_Pa calibrated pressure in Pa returned to caller
 *
 * @return None
 *
 */
void qmp6989_compensation_fixed_point_s64(int16_t int16_tT, int32_t int32_tP, int16_t int16_tValue[], uint8_t uint8_tPower[], int32_t* pint32_tT_Celsius, int32_t* pint32_tP_Pa){

  s64 tmp, val, s64T, s64P;
  s64T = int16_tT;
  s64P = int32_tP;

  //Temperature
  *pint32_tT_Celsius = int16_tT;

  //Pressure
  val = 0;
  //beta0
  tmp = int16_tValue[0] * QMP6989_POWER_SCALE[uint8_tPower[0]] * 10;
  val += tmp;
  //beta1*T
  tmp = s64T * int16_tValue[1];
  tmp = tmp * QMP6989_POWER_SCALE[uint8_tPower[1]];
  tmp = RoundDivide(tmp, 10000);
  val += tmp;
  //beta2*T*T
  tmp = s64T * int16_tValue[2];
  tmp = tmp * s64T;
  tmp = tmp * QMP6989_POWER_SCALE[uint8_tPower[2]];
  tmp = RoundDivide(tmp, 1000000000);
  val += tmp;
  //beta3*P
  tmp = s64P * int16_tValue[3];
  tmp = tmp * QMP6989_POWER_SCALE[uint8_tPower[3]];
  tmp = RoundDivide(tmp, 10000);
  val += tmp;
  //beta4*P*T
  tmp = s64P * int16_tValue[4];
  tmp = tmp * s64T;
  tmp = tmp * QMP6989_POWER_SCALE[uint8_tPower[4]];
  tmp = RoundDivide(tmp, 1000000000);
  val += tmp;
  //beta5*P*T*T
  tmp = s64P * int16_tValue[5];
  tmp = tmp * s64T;
  tmp = ShiftRight(tmp, 10) * s64T;
  tmp = ShiftRight(tmp, 10) * QMP6989_POWER_SCALE[uint8_tPower[5]];
  tmp = RoundDivide(tmp, 95367432);
  val += tmp;
  //beta6*P*P
  tmp = s64P * int16_tValue[6];
  tmp = tmp * s64P;
  tmp = ShiftRight(tmp, 7) * QMP6989_POWER_SCALE[uint8_tPower[6]];
  tmp = RoundDivide(tmp, 781250000);
  val += tmp;
  //beta7*P*P*T
  tmp = s64P * int16_tValue[7];
  tmp = tmp * s64P;
  tmp = ShiftRight(tmp, 10) * s64T;
  tmp = ShiftRight(tmp, 10) * QMP6989_POWER_SCALE[uint8_tPower[7]];
  tmp = RoundDivide(tmp, 9536743164);
  val += tmp;
  //beta8*P*P*T*T
  tmp = s64P * int16_tValue[8];
  tmp = tmp * s64P;
  tmp = ShiftRight(tmp, 9) * ShiftRight(s64T, 1);
  tmp = ShiftRight(tmp, 12) * ShiftRight(s64T, 3);
  tmp = ShiftRight(tmp, 7) * QMP6989_POWER_SCALE[uint8_tPower[8]];
  tmp = RoundDivide(tmp, 23283064365);
  val += tmp;

  *pint32_tP_Pa = (int32_t)RoundDivide(val, 10);

  return;
}
#endif

/*!
 * @brief qmp6989 set pressure OSR
 *
 * @param osrP OSR to set
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
int8_t qmp6989_set_P_OSR(QMP6989_P_OSR_Type osrP){

  int8_t comRslt = 0, int8_tTmp;
  uint8_t uint8_tData;

  //Read A6h
  int8_tTmp = qmp6989_burst_read(QMP6989_REG_CONFIG2, &uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  //Set the A6h[2:0] OSR bits
  uint8_tData = QMP6989_SET_BITSLICE(uint8_tData, QMP6989_P_OSR, osrP);
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CONFIG2, &uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

 EXIT:
  return comRslt;
}


/*!
 * @brief qmp6989 set temperature OSR
 *
 * @param osrT OSR to set
 *
 * @return Result from bus communication function
 * @retval -1 Bus communication error
 * @retval -127 Error null bus
 *
 */
int8_t qmp6989_set_T_OSR(QMP6989_T_OSR_Type osrT){

  int8_t comRslt = 0, int8_tTmp;
  uint8_t uint8_tData;

  //Read A7h
  int8_tTmp = qmp6989_burst_read(QMP6989_REG_CONFIG3, &uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

  //Set the A7h[2:0] OSR bits
  uint8_tData = QMP6989_SET_BITSLICE(uint8_tData, QMP6989_T_OSR, osrT);
  int8_tTmp = qmp6989_burst_write(QMP6989_REG_CONFIG3, &uint8_tData, 1);

  if(int8_tTmp < 0){ //communication error
    comRslt = int8_tTmp;
    goto EXIT;
  }
  comRslt += int8_tTmp;

 EXIT:
  return comRslt;
}

void qmp6989_get_data(float *press, float *temp){
	qmp6989_measure_P(&int32_tP);
	qmp6989_measure_T(&int16_tT);

#ifdef FLOAT_SUPPORT
	qmp6989_compensation(int16_tT, int32_tP, fCalibParam, &fT_Celsius, &fP_Pa);
	*press = fP_Pa;
	*temp = fT_Celsius;
#else
	qmp6989_compensation_fixed_point_s64(int16_tT, int32_tP, int16_tValue, uint8_tPower, &int32_tT_Celsius, &int32_tP_Pa);
	*press = int32_tP_Pa;
	*temp = int32_tT_Celsius/256.0;
#endif
}

//static qmp6989_sensor qmp6989_curr_sensor = 0;

void qmp6989_set_mode(qmp6989_mode mode)
{
	//int8_t comRslt = 0;
	uint8_t uint8_tData[3];

	qmp6989_curr_mode = mode;
	if(mode == QMP6989_MODE_P)
	{
	  uint8_tData[0] = 0x02;	// Set A5h = 0x02, raw data out
	}
	else if(mode == QMP6989_MODE_T)
	{
		uint8_tData[0] = 0x00;	// Set A5h = 0x00, Calibrated data out
	}
	qmp6989_burst_write(QMP6989_REG_CONFIG1, uint8_tData, 1);
	// Set 30h = 0x08, T-Forced mode, Set 30h = 0x09, P-Forced mode
	uint8_tData[0] = (uint8_t)mode;
	qmp6989_burst_write(QMP6989_REG_CMD, uint8_tData, 1);
}


void qmp6989_get_data_ext(float *press, float *temp)
{
	static uint8_t qmp6989_data_i = 0;

	uint8_t uint8_tData[3]={0x00,0x00,0x00};
	uint8_t int8_ttimecnt=0;

	qmp6989_burst_read(QMP6989_REG_STATUS, uint8_tData, 1);
	while(!(uint8_tData[0]&0x01)&&(int8_ttimecnt++<20))
	{
		qmp6989_delay1ms(1);
		qmp6989_burst_read(QMP6989_REG_STATUS, uint8_tData, 1);
	}

	if(uint8_tData[0]&0x01)
	{
		if(qmp6989_curr_mode == QMP6989_MODE_P)
		{
			qmp6989_burst_read(QMP6989_REG_PRESSH, uint8_tData, 3);
#if 0
			int8_tTmp = sizeof(int32_tP)*8 - 24;
			// Get the raw pressure in code
			int32_tP = (int32_t)(((int32_t)uint8_tData[0] << 16) + ((int32_t)uint8_tData[1] << 8) + uint8_tData[2]);
			int32_tP = (int32_tP << int8_tTmp) >> int8_tTmp; //24 bit sign extension
#else
			int32_tP = (int32_t)(((int32_t)uint8_tData[0] << 24) + ((int32_t)uint8_tData[1] << 16) + ((int32_t)uint8_tData[2]<<8));
			int32_tP = int32_tP >> 8;
#endif
		}
		else if(qmp6989_curr_mode == QMP6989_MODE_T)
		{
			// Read 09h~0Ah
			qmp6989_burst_read(QMP6989_REG_TEMPH, uint8_tData, 2);
			// Get the calibrated temperature in code
			int16_tT = (int16_t)(((int16_t)uint8_tData[0] << 8) + uint8_tData[1]);
		}

		if((++qmp6989_data_i)>=5)
		{
			qmp6989_data_i = 0;
			qmp6989_set_mode(QMP6989_MODE_T);
		}
		else
		{
			qmp6989_set_mode(QMP6989_MODE_P);
		}
	}
	else
	{
		qmp6989_set_mode(QMP6989_MODE_P);
	}

#ifdef FLOAT_SUPPORT
	qmp6989_compensation(int16_tT, int32_tP, fCalibParam, &fT_Celsius, &fP_Pa);
	*press = fP_Pa;
	*temp = fT_Celsius;
#else
	qmp6989_compensation_fixed_point_s64(int16_tT, int32_tP, int16_tValue, uint8_tPower, &int32_tT_Celsius, &int32_tP_Pa);
	*press = int32_tP_Pa;
	*temp = int32_tT_Celsius/256.0;
#endif
}

void vQMP6989ValueGet(float *fPressure, float *fTemperature)
{
    qmp6989_get_data(fPressure, fTemperature);

    *fTemperature = (*fTemperature - 32.0f) / 1.8f;
}
